#include "modules/CtrlModule/ActivePlanner/ObsFollowPlanner.h"
#include "modules/CtrlModule/ActivePlanner/NaviPlanner.h"
#include "lib/Time.h"

namespace behavior_controller
{
ObsFollowPlanner obs_follow_planner;

void ObsFollowPlanner::run(double &linear_vel, double &angular_vel, bool &finished,
                           std::shared_ptr<PurePursuitControllerD> &controllerF)
{
    if (!online_flag)
    {
        followPartRoom(linear_vel, angular_vel, finished, controllerF);
    }
    else
    {
        followRoom(linear_vel, angular_vel, finished);
    }
}

void ObsFollowPlanner::followRoom(double &linear_vel, double &angular_vel, bool &finished)
{
    //linear为�?�向前，angular为�?�向�?
    if (dis_ >= MARGIN_TURNDIST) //右侧无障碍物的情况右�?
    {
        linear_vel = 0.2;
        angular_vel = t_angular1;
        total_err =
        last_dis_p = 0;
    } else if (dis_ >= MARGIN_LEFT && dis_ < MARGIN_TURNDIST) //右侧有障碍物，但距�?�还有些远的情况
    {
        linear_vel = 0.2;
        angular_vel = t_angular2;
        total_err =
        last_dis_p = 0;
    } else if (dis_ >= MARGIN_RIGHT && dis_ < MARGIN_LEFT) //沿边时需要调整的情况
    {
        pid_adjustment(linear_vel, angular_vel);
    } else
    {
        linear_vel = 0.2;
        angular_vel = t_angular3;
        total_err =
        last_dis_p = 0;
    }
    if ((pow(DataSet::robot_pose.x() - start_pose.pose.position.x, 2) +
         pow(DataSet::robot_pose.y() - start_pose.pose.position.y, 2)) <= 0.09
        && Time::Now() - start_pose.header.stamp.sec > 120)  // 120s   , 0.3m -> 0.3*0.3=0.09
    {
        finished = true;
    }
}

void ObsFollowPlanner::followVirtualWall(double &linear_vel, double &angular_vel, bool &finished,
                                         std::shared_ptr<PurePursuitControllerD> &controllerF)
{
    geometry_msgs::msg::Twist twist;
    controllerF->computeVelocityCommands(geometry_msgs::msg::PoseStamped(), twist);
    linear_vel = twist.linear.x;
    angular_vel = twist.angular.z;
    if (controllerF->FinishedVirtualWall(ROBOT_DIAMETER))
    {
        control_flag = false;
    }
}

void ObsFollowPlanner::followPartRoom(double &linear_vel, double &angular_vel, bool &finished,
                                      std::shared_ptr<PurePursuitControllerD> &controllerF)
{
    static bool done_flag = false;
    if (!room_init)
    {
        room_init = true;
        wallPtr = &navi_planner.ipaPlanner()->getVirtualWall();
    }
    if (!control_flag)
    {
//        std::cout << "real wall !" << std::endl;
        tf2::Quaternion quaternion_;
        geometry_msgs::msg::PoseStamped pose_msg;
        for (auto & i : *wallPtr)
        {
            if ((pow(DataSet::robot_pose.x() - i[0].pose.position.x, 2) +
                 pow(DataSet::robot_pose.y() - i[0].pose.position.y, 2)) <= 0.04) // 0.2*0.2= 0.04
            {
                control_flag = true;
                done_flag = true;
                //std::cout<< "plan point num= "<< wallPtr->at(i).size()<<std::endl;
                nav_msgs::msg::Path path_;
                path_.header.frame_id = "map";
                path_.header.stamp = navi_planner.getNode()->now();
                path_.poses.push_back(getRobotPose());
                quaternion_.setRPY(0, 0, atan2(
                        i.begin()->pose.position.y - path_.poses[0].pose.position.y,
                        i.begin()->pose.position.x - path_.poses[0].pose.position.x));
                pose_msg.pose.orientation.x = quaternion_.x();
                pose_msg.pose.orientation.y = quaternion_.y();
                pose_msg.pose.orientation.z = quaternion_.z();
                pose_msg.pose.orientation.w = quaternion_.w();
                pose_msg.pose.position = path_.poses[0].pose.position;
                path_.poses.push_back(pose_msg);
                pose_msg.pose.position = i.begin()->pose.position;
                path_.poses.push_back(pose_msg);
                path_.poses.insert(path_.poses.end(), i.begin(), i.end());
                controllerF->setPlan(path_);
                break;
            }
        }
        followRoom(linear_vel, angular_vel, finished);
    }
    else
    {
        std::cout << "virtual wall !" << std::endl;

        /* for(int i=0; i<wallPtr->size();i++ )
        {
            if((pow(DataSet::robot_pose.x() -wallPtr->at(i)[wallPtr->at(i).size()-1].pose.position.x,2)+
                        pow(DataSet::robot_pose.y()-wallPtr->at(i)[wallPtr->at(i).size()-1].pose.position.y,1)) <= 0.04) // 0.1*0.1= 0.04
            {
                control_flag=false;
                done_flag=false;
                break;
            }
        } */
        followVirtualWall(linear_vel, angular_vel, finished, controllerF);
    }
}

void ObsFollowPlanner::pid_adjustment(double &linear_vel, double &angular_vel)
{
    linear_vel = 0.2;
    double dis_p = dis_ - MARGIN_OBSDIST;       //当dis_p>0时，应向右转
    double dis_d = dis_p - last_dis_p;        //当dis_d>0时，说明变化趋势在�?�大，应反作用于角速度
    total_err += dis_p;
    angular_vel = -(dis_p * MARGIN_P + total_err * MARGIN_I +
                    dis_d * MARGIN_D); //角速度输出，需要调节p d参数，使机器人�?�走轨迹不至于过于晃动�?
//    std::cout << "PID  dis_= " << dis_ << " dis_p=" << dis_p
//              << " last_dis_p=" << last_dis_p << " angular_vel= " << angular_vel << std::endl;
    last_dis_p = dis_p;
}


} // namespace behavior_controller